In [1]:
import os
import numpy as np
import cv2
import json
# ------------------------------
# ENTER YOUR REQUIREMENTS HERE:
ARUCO_DICT = cv2.aruco.DICT_4X4_50
SQUARES_VERTICALLY = 5
SQUARES_HORIZONTALLY = 7
#SQUARE_LENGTH = 0.03 # in meters
#MARKER_LENGTH = 0.015 # in meters
SQUARE_LENGTH = 0.10 # in meters
MARKER_LENGTH = 0.08 # in meters
# ...
PATH_TO_YOUR_IMAGES = 'testdata/charuco_calib_set_1/'
print(f"{ARUCO_DICT=}")
# ------------------------------
json_file_path = './calibration.json'
with open(json_file_path, 'r') as file: # Read the JSON file
json_data = json.load(file)
mtx = np.array(json_data['mtx'])
dst = np.array(json_data['dist'])
dictionary = cv2.aruco.getPredefinedDictionary(ARUCO_DICT)
board = cv2.aruco.CharucoBoard((SQUARES_VERTICALLY, SQUARES_HORIZONTALLY), SQUARE_LENGTH, MARKER_LENGTH, dictionary)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, params)
ARUCO_DICT=0
In [2]:
image_paths = [os.path.join(PATH_TO_YOUR_IMAGES, f) for f in os.listdir(PATH_TO_YOUR_IMAGES) if f.endswith(".JPG")]
img_path = image_paths[10] # select image
image_color = cv2.imread(img_path)
image = cv2.cvtColor(image_color, cv2.COLOR_BGR2GRAY)
h, w = image.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dst, (w,h), 1, (w,h))
image = cv2.undistort(image, mtx, dst, None, newcameramtx)
all_charuco_ids = []
all_charuco_corners = []
marker_corners, marker_ids, rejectedCandidates = detector.detectMarkers(image)
if marker_ids is not None and len(marker_ids) > 0: # If at least one marker is detected
# cv2.aruco.drawDetectedMarkers(image_copy, marker_corners, marker_ids)
ret, charucoCorners, charucoIds = cv2.aruco.interpolateCornersCharuco(marker_corners, marker_ids, image, board)
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(charucoCorners, charucoIds, board, np.array(mtx), np.array(dst), np.empty(1), np.empty(1))
Zx, Zy, Zz = tvec[0][0], tvec[1][0], tvec[2][0]
fx, fy = mtx[0][0], mtx[1][1]
else:
print("no marker detected")
In [3]:
%matplotlib widget
In [4]:
import matplotlib.pyplot as plt
plt.figure()
plt.imshow(cv2.cvtColor(image_color, cv2.COLOR_BGR2RGB))
plt.scatter(np.hstack(marker_corners)[0,:,0],np.hstack(marker_corners)[0,:,1],label="marker Corners")
plt.scatter(charucoCorners[:,0,0],charucoCorners[:,0,1], label="charuco Corners")
plt.legend()
Out[4]:
<matplotlib.legend.Legend at 0x7f9a9043f3e0>
In [5]:
rotation_matrix, _ = cv2.Rodrigues(rvec)
# Camera pose: the inverse of the extrinsic matrix (rotation + translation)
camera_pose = np.eye(4)
camera_pose[:3, :3] = rotation_matrix
camera_pose[:3, 3] = tvec.flatten()
camera_mat = np.eye(4)
camera_mat[:3,:3] = mtx
P = camera_mat @ camera_pose
P = P[:3,:] # we only had 4 rows for multiplication before
world_corners = board.getChessboardCorners()
image_pts = P @ np.hstack((world_corners,np.ones((world_corners.shape[0],1)))).T
# recover homogeneous coordinates
image_pts = image_pts / image_pts[-1,:]
image_pts = image_pts[:2,:]
image_pts.T
Out[5]:
array([[3496.29498882, 2637.96890187],
[3404.36141996, 2147.46211359],
[3315.40360475, 1672.83230461],
[3229.27938211, 1213.32098179],
[3820.67235209, 2546.00032368],
[3731.34049899, 2096.91181377],
[3644.66772727, 1661.19102043],
[3560.53706993, 1238.24992796],
[4095.40790656, 2468.10636071],
[4008.97857633, 2053.98953234],
[3924.930495 , 1651.28220165],
[3843.16660774, 1259.51934085],
[4331.08717634, 2401.285769 ],
[4247.6603771 , 2017.08981195],
[4166.37291348, 1642.74589413],
[4087.14355537, 1277.8799357 ],
[4535.48583493, 2343.33396303],
[4455.04753566, 1985.02817953],
[4376.53833326, 1635.31539926],
[4299.88967062, 1293.89024021],
[4714.44396671, 2292.59514214],
[4636.9162088 , 1956.91165299],
[4561.13464298, 1628.788912 ],
[4487.04094723, 1307.97439364]])
In [6]:
import matplotlib.pyplot as plt
plt.figure()
plt.imshow(cv2.cvtColor(image_color, cv2.COLOR_BGR2RGB))
plt.scatter(np.hstack(marker_corners)[0,:,0],np.hstack(marker_corners)[0,:,1],label="marker Corners")
plt.scatter(charucoCorners[:,0,0],charucoCorners[:,0,1], label="charuco Corners")
plt.scatter(image_pts[0,:],image_pts[1,:], label="reprojected Corners")
plt.legend()
Out[6]:
<matplotlib.legend.Legend at 0x7f9a8e1fbb00>
In [7]:
ic = cv2.drawFrameAxes(image_color.copy(), mtx,dst, rvec, tvec, 0.25, 25)
plt.figure()
plt.imshow(ic)
plt.show()
successfully overlaid corners mean that returned rvec and tvec are indeed the camera pose. Note that because of opencv the image origin is top left
In [8]:
image_pts
Out[8]:
array([[3496.29498882, 3404.36141996, 3315.40360475, 3229.27938211,
3820.67235209, 3731.34049899, 3644.66772727, 3560.53706993,
4095.40790656, 4008.97857633, 3924.930495 , 3843.16660774,
4331.08717634, 4247.6603771 , 4166.37291348, 4087.14355537,
4535.48583493, 4455.04753566, 4376.53833326, 4299.88967062,
4714.44396671, 4636.9162088 , 4561.13464298, 4487.04094723],
[2637.96890187, 2147.46211359, 1672.83230461, 1213.32098179,
2546.00032368, 2096.91181377, 1661.19102043, 1238.24992796,
2468.10636071, 2053.98953234, 1651.28220165, 1259.51934085,
2401.285769 , 2017.08981195, 1642.74589413, 1277.8799357 ,
2343.33396303, 1985.02817953, 1635.31539926, 1293.89024021,
2292.59514214, 1956.91165299, 1628.788912 , 1307.97439364]])
In [ ]:
In [9]:
camera_pose
Out[9]:
array([[-0.16105014, 0.89798268, -0.40949965, 0.11415069],
[-0.98380074, -0.11296621, 0.13919315, 0.20722587],
[ 0.07873341, 0.42528313, 0.90162925, 0.42033198],
[ 0. , 0. , 0. , 1. ]])
In [ ]:
In [ ]:
In [ ]:
In [10]:
import plotly.graph_objects as go
from stl import mesh
def plot_3d_points_and_camera_centers(points, extrinsic_matrices):
"""
Plots a 3D scatter plot of the given points and camera representations from extrinsic matrices.
Parameters:
- points: A 3xN NumPy array, where each column represents a point (x, y, z).
- extrinsic_matrices: A list of 4x4 NumPy arrays representing the extrinsic matrices for each camera.
"""
# Ensure points is a NumPy array
points = np.asarray(points)
# Check if the input is a 3xN array
if points.shape[0] != 3:
raise ValueError("Input points must be a 3xN NumPy array.")
# Extract x, y, z coordinates
x = points[0, :]
y = points[1, :]
z = points[2, :]
# Create a 3D scatter plot for the points
fig = go.Figure(data=[go.Scatter3d(
x=x,
y=y,
z=z,
mode='markers',
marker=dict(
size=5,
color='blue', # Marker color for points
opacity=0.8
),
name='3D Points'
)])
# Function to create a cone shape for the camera representation
def create_camera_cone(extrinsic):
# Transform the cone points using the extrinsic matrix
camera_center = extrinsic[:3, 3]
rotation_matrix = extrinsic[:3, :3]
def rotate_and_translate(vertices, rotation_matrix, translation_vector):
# Apply rotation
rotated_vertices = vertices @ rotation_matrix.T # Ensure correct shape
# Apply translation
translated_vertices = rotated_vertices + translation_vector
return translated_vertices
# Load the STL file
your_mesh = mesh.Mesh.from_file('pyramid.stl')
# Extract vertices
vertices = your_mesh.vectors.reshape(-1, 3)
# Rotate and translate the frustum
transformed_vertices = rotate_and_translate(vertices, rotation_matrix, camera_center)
# compute forward vector
frustum = go.Mesh3d(
x=transformed_vertices[:, 0],
y=transformed_vertices[:, 1],
z=transformed_vertices[:, 2],
opacity=0.75,
color='blue'
)
return frustum
# Plot camera representations from extrinsic matrices
for idx, extrinsic in enumerate(extrinsic_matrices):
# Create the camera cone
cone_points = create_camera_cone(extrinsic)
# Add the cone to the plot
fig.add_trace(cone_points)
# Set the layout
fig.update_layout(
title='3D Scatter Plot of Points and Camera Representations',
scene=dict(
xaxis_title='X Axis',
yaxis_title='Y Axis',
zaxis_title='Z Axis'
)
)
# Show the figure in the Jupyter Notebook
fig.show()
# Define a list of extrinsic matrices (4x4)
extrinsic_matrices = [
camera_pose
]
# Call the function to plot the points and camera representations
plot_3d_points_and_camera_centers(world_corners.T, extrinsic_matrices)
In [11]:
camera_pose
Out[11]:
array([[-0.16105014, 0.89798268, -0.40949965, 0.11415069],
[-0.98380074, -0.11296621, 0.13919315, 0.20722587],
[ 0.07873341, 0.42528313, 0.90162925, 0.42033198],
[ 0. , 0. , 0. , 1. ]])
now use validated code to estimate position of all cameras:
In [12]:
def reproject_world_points(rvec, tvec, mtx, world_corners):
rotation_matrix, _ = cv2.Rodrigues(rvec)
# Camera pose: the inverse of the extrinsic matrix (rotation + translation)
camera_pose = np.eye(4)
camera_pose[:3, :3] = rotation_matrix
camera_pose[:3, 3] = tvec.flatten()
camera_mat = np.eye(4)
camera_mat[:3,:3] = mtx
P = camera_mat @ camera_pose
P = P[:3,:] # we only had 4 rows for multiplication before
image_pts = P @ np.hstack((world_corners,np.ones((world_corners.shape[0],1)))).T
# recover homogeneous coordinates
image_pts = image_pts / image_pts[-1,:]
image_pts = image_pts[:2,:]
return camera_pose, P, image_pts
def estimate_pose(image_color, mtx, dst, detector):
image = cv2.cvtColor(image_color, cv2.COLOR_BGR2GRAY)
h, w = image.shape[:2]
newcameramtx, roi = cv2.getOptimalNewCameraMatrix(mtx, dst, (w,h), 1, (w,h))
image = cv2.undistort(image, mtx, dst, None, newcameramtx)
all_charuco_ids = []
all_charuco_corners = []
marker_corners, marker_ids, rejectedCandidates = detector.detectMarkers(image)
if marker_ids is not None and len(marker_ids) > 0: # If at least one marker is detected
# cv2.aruco.drawDetectedMarkers(image_copy, marker_corners, marker_ids)
ret, charucoCorners, charucoIds = cv2.aruco.interpolateCornersCharuco(marker_corners, marker_ids, image, board)
retval, rvec, tvec = cv2.aruco.estimatePoseCharucoBoard(charucoCorners, charucoIds, board, np.array(mtx), np.array(dst), np.empty(1), np.empty(1))
Zx, Zy, Zz = tvec[0][0], tvec[1][0], tvec[2][0]
fx, fy = mtx[0][0], mtx[1][1]
if retval == 0:
return None
return rvec, tvec, charucoCorners
else:
return None
#print("no marker detected")
In [13]:
from scipy.spatial import cKDTree
def compute_reprojection_error(reprojected_points, detected_points):
# Create a KDTree for fast nearest neighbor search
tree = cKDTree(detected_points)
# Find the nearest detected point for each reprojected point
distances, indices = tree.query(reprojected_points)
# Calculate the reprojection error as the mean distance
reprojection_error = np.mean(distances)
return reprojection_error
In [14]:
def invert_transform(rotvec, tvec):
# Step 1: Convert rotation vector to rotation matrix
R, _ = cv2.Rodrigues(rotvec)
# Step 2: Compute the inverse of the rotation matrix
R_inv = R.T
# Step 3: Convert the inverse rotation matrix back to a rotation vector
rotvec_inv, _ = cv2.Rodrigues(R_inv)
# Step 4: Compute the inverse translation vector
tvec_inv = -R_inv @ tvec
return rotvec_inv, tvec_inv
def draw_reproj(image_color, charucoCorners, image_pts):
f = plt.figure()
ax = f.add_subplot(111)
ax.imshow(cv2.cvtColor(image_color, cv2.COLOR_BGR2RGB))
#plt.scatter(np.hstack(marker_corners)[0,:,0],np.hstack(marker_corners)[0,:,1],label="marker Corners")
ax.scatter(charucoCorners[:,0,0],charucoCorners[:,0,1], label="charuco Corners")
ax.scatter(image_pts[0,:],image_pts[1,:], label="reprojected Corners")
ax.legend()
plt.close(f)
return f
def convert_left_to_right(rvec, tvec):
# Convert rotation vector to rotation matrix
R, _ = cv2.Rodrigues(rvec)
# Invert the z-axis for the rotation matrix
R[:, 2] = -R[:, 2]
# Convert back to rotation vector
rvec_right = cv2.Rodrigues(R)[0]
# Invert the z-component of the translation vector
tvec_right = tvec.copy()
tvec_right[2] = -tvec_right[2]
return rvec_right, tvec_right
In [15]:
REPROJECTION_ERROR_THRESH = 10000.0
dictionary = cv2.aruco.getPredefinedDictionary(ARUCO_DICT)
board = cv2.aruco.CharucoBoard((SQUARES_VERTICALLY, SQUARES_HORIZONTALLY), SQUARE_LENGTH, MARKER_LENGTH, dictionary)
params = cv2.aruco.DetectorParameters()
detector = cv2.aruco.ArucoDetector(dictionary, params)
world_corners = board.getChessboardCorners()
camera_poses = []
figures = []
for i, image_path in enumerate(image_paths):
#img_path = image_paths[12] # select image
#print(image_path)
image_color = cv2.imread(image_path)
res = estimate_pose(image_color, mtx, dst, detector)
if res == None:
print("pose could not be estimated")
rvec, tvec, charuco_corners = res
#rvec, tvec = convert_left_to_right(rvec, tvec)
#rvec, tvec = invert_transform(rvec, tvec)
E, P, img_pts = reproject_world_points(rvec, tvec, mtx, world_corners)
#print(charuco_corners[:2,0,:])
repr_err = compute_reprojection_error(charuco_corners, img_pts.T)
if repr_err < REPROJECTION_ERROR_THRESH:
camera_poses.append(E)
figures.append(draw_reproj(image_color, charuco_corners, img_pts))
print(i, image_path,repr_err)
plot_3d_points_and_camera_centers(world_corners.T, camera_poses)
0 testdata/charuco_calib_set_1/GOPR0016.JPG 2.4047658584014737 1 testdata/charuco_calib_set_1/GOPR0017.JPG 1.1539104109021663 2 testdata/charuco_calib_set_1/GOPR0018.JPG 1.5094955400676695 3 testdata/charuco_calib_set_1/GOPR0019.JPG 2.9881964206864953 4 testdata/charuco_calib_set_1/GOPR0020.JPG 4.304995267616654 5 testdata/charuco_calib_set_1/GOPR0021.JPG 3.0850779752409814 6 testdata/charuco_calib_set_1/GOPR0022.JPG 2.330988109181218 7 testdata/charuco_calib_set_1/GOPR0023.JPG 3.756626638282929 8 testdata/charuco_calib_set_1/GOPR0024.JPG 4.851068595352484 9 testdata/charuco_calib_set_1/GOPR0025.JPG 3.510577565476059 10 testdata/charuco_calib_set_1/GOPR0026.JPG 5.559737925918886 11 testdata/charuco_calib_set_1/GOPR0027.JPG 7.608089775930453 12 testdata/charuco_calib_set_1/GOPR0028.JPG 1.6177716689985049 13 testdata/charuco_calib_set_1/GOPR0029.JPG 2.784991333966658
note that no and especially not trace 11 (indexed by 11) is located in a way that could explain image GOPR0026. Tt should be next to the board but it is displayed above.
In [16]:
#camera_poses[10:12]
figures[10]
Out[16]:
In [ ]: